#!/usr/bin/env python
#coding:utf-8
#roslaunch turtlebot_teleop keyboard_teleop.launch

import struct 
import binascii
import serial
import math

import rospy

from geometry_msgs.msg import Twist


#typedef struct hd_robot_protocol
#{
#	int8_t   type[3];			//the type of device, example dq2 
#	int8_t   move_mode;			//move mode:0-linear motion,1-rotation motion,2-stop,
#								//3-四轮单独驱动
#	int16_t  start_dir;			//Direction of departure:0~360(-180 ~ 180)
#	int16_t  speed[4];			//line speed:mm/s,max: 3.0*1000mm/s	hereis m1,m2,m3,m4
#   char	 unused;			//unused.
#	uint64_t verge;				//校验和
#	char     end;				//结束符号，0xaa
#}hd_robot_protocol;

class HDDiscoveryQ2:
	def __init__(self):
		self.q2 = struct.Struct('<3sbh4hBHB')
		self.type	   = 'dq2'
		self.move_mode = 0
		self.start_dir = 0
		self.speed	   = 0	#if move_mode != 3, use speed!
		self.flm	   = 0	#front left  motor
		self.frm	   = 0	#front right motor
		self.blm	   = 0	#back  left  motor
		self.brm	   = 0	#back  right motor
		self.unused	   = 0
		self.verge	   = 0
		self.end	   = 0xaa

	def pack(self):

		if self.move_mode == 3:
			#这里的代码是硬件相关，四个轮子（下标从0开始）
			#物理位置机器转动方向如图所示
			#  3/\    2\/
			#  					
			#
			#  0/\    1\/
			m=4*[0]
			m[3] = -self.flm
			m[2] =  self.frm
			m[0] = -self.blm
			m[1] =  self.brm
			data = self.q2.pack( self.type, self.move_mode, 0, \
								 m[0],m[1],m[2],m[3], \
								 self.unused, 0, 0 )
		
			checksum = 0
			for i in range( len(data) ):
				x, = struct.unpack('<B', data[i])
				checksum+=int(x)
			self.verge = checksum
			return self.q2.pack( self.type, self.move_mode, 0, \
								 m[0],m[1],m[2],m[3], \
								 self.unused, self.verge, self.end )
		#if move_mode != 3:
		#兼容以前老算法
		data = self.q2.pack( self.type, self.move_mode, self.start_dir, \
							 self.speed, 0, 0, 0, \
							 self.unused, 0, 0 )
		
		checksum = 0
		for i in range( len(data) ):
			x, = struct.unpack('<B', data[i])
			checksum+=int(x)
		self.verge = checksum
		return self.q2.pack( self.type, self.move_mode, self.start_dir, \
							 self.speed, 0, 0, 0, \
							 self.unused, self.verge, self.end )


def stop():
	#stop the q2 platform
	a = 0
	



#current status




def hd_q2_callback(msg):
	global before
	#the format of msg refer to: http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Twist.html
	#msg.linear.x
	#msg.linear.y
	#msg.linear.z

	#msg.angular.x
	#msg.angular.y
	#msg.angular.z
	
	now = rospy.Time.now()
	duration = now - before

	#print duration.secs, duration.nsecs / (1000*1000)

	if duration.secs <= 0 and ( duration.nsecs/(1000*1000) ) <= 100:
		return 
	
	before = rospy.Time.now()

	rospy.loginfo("Received a /cmd_vel message!")
	rospy.loginfo("Linear Components: [%f, %f, %f]"%(msg.linear.x, msg.linear.y, msg.linear.z))
	rospy.loginfo("Angular Components: [%f, %f, %f]"%(msg.angular.x, msg.angular.y, msg.angular.z))

	#stop
	if     msg.linear.x  == 0.0 \
	   and msg.linear.y  == 0.0 \
	   and msg.linear.z == 0.0 \
	   and msg.angular.z == 0.0 :
	   
		q2 = HDDiscoveryQ2()
		q2.move_mode = 2
		serial_port.write( q2.pack() )
		return 

	#现在只考虑msg.linear.y == 0的情况，其他情况不考虑。
	
	
	q2 = HDDiscoveryQ2()
	q2.move_mode = 3
	
	x = msg.linear.x 
	y = msg.linear.y
	z = msg.angular.z
	
	ab = 0.134+0.134		#车子几何中心到轮子的距离（单位米）
							#事实上，还要考虑轮子的半径。
							#	因要轮子的转速是由轮子的半径和轮子轴心角速度决定的。
							#	上层控制轮子的转速，只需要控制轮子的角速度即可，这里把
							#	轮子的角速度乘以轮子的半径就可以算出速度。
							#然而，q2开发平台固件向上提供接口并不是控制轮子的角速度，而是控制
							#轮子的速度，所以轮子半径等在这里不作考虑。
	flm = int((x + y - z*ab)*1000)
	frm = int((x - y + z*ab)*1000)

	blm = int((x - y - z*ab)*1000)
	brm = int((x + y + z*ab)*1000)
	

	if flm > 2500:
		flm = 2500
	if flm <-2500:
		flm =-2500

	if frm > 2500:
		frm = 2500
	if flm <-2500:
		flm =-2500

	if blm > 2500:
		blm = 2500
	if blm <-2500:
		blm =-2500

	if brm > 2500:
		brm = 2500
	if brm <-2500:
		brm =-2500

	q2.flm = flm
	q2.frm = frm
	q2.blm = blm
	q2.brm = brm
	
	
	serial_port.write( q2.pack() )
	
	#此处无条件返回。
	return 
	#straight
	#About the direction
	#x = 0  (y=0 and x > 0)
	#x = 180 (y=0 and x < 0)
	#x = 90 pi (x=0 & y>0 )
	#x = 270 pi (x=0 & y<0 )
	#x = arctan( y/x ) ( y>0  )
	#x = 180 + arctan( y/x ) < y<0 )

	if msg.linear.y == 0.0:
		if msg.linear.x >= 0.0:
			x = 90
			#print 'UP'
		else:
			#print 'Down'
			x = 270
		q2 = HDDiscoveryQ2()
		q2.move_mode = 0
		q2.start_dir = x
		speed = abs(msg.linear.x)
		
		#print speed
		
		q2.speed	= int(1000*speed)
		if q2.speed >= 2.5*1000:
			q2.speed = 2.5*1000
		
		serial_port.write( q2.pack() )
		return 

	
	
	return 


if __name__ == '__main__':
	global serial_port
	global slow
	global before
	
	
	slow = 0
	rospy.init_node('discovery_q2')

	before = rospy.Time.now()

	rospy.Subscriber( "/cmd_vel" , Twist, hd_q2_callback )
	rospy.Subscriber( "/cmd_vel_mux/input/teleop" , Twist, hd_q2_callback )

	serial_port = serial.Serial( '/dev/discovery_q2', 9600 )

	#port = rospy.getParam("port_name")
	rospy.spin()
	
	
